#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
苏翎菲--Formation optimal tracking for open heterogenous multiagent systems
无人机、无人车-XGC控制程序 in python3
Author: 张镕麒
time:   2025.06.28
论文试验
"""
from __future__ import division
from __future__ import print_function
# make code consistent with python3 (including 1/2=0.5, print())

import os
import time
from datetime import datetime
import numpy as np
import math
import rospy
from std_msgs.msg import Int32, Float32, Empty
from nav_msgs.msg import Odometry
# import tf
from geometry_msgs.msg import Twist, Point, Pose, Quaternion, Vector3, PoseStamped, TwistStamped
from sensor_msgs.msg import Imu
# import json
from scipy.linalg import solve_continuous_are
from mavros_msgs.msg import AttitudeTarget, PositionTarget, State
from mavros_msgs.srv import CommandBool, SetMode
from scipy.io import loadmat
import scipy.io as sio
from scipy.optimize import minimize


class zrq_control:
    def __init__(self, log_folder):
        ############# initialize #############
        self.if_sim = 0 # 1 是仿真//0 是exp
        self.log_folder = log_folder
        self.f = 100.0   # 程序频率
        self.dt = 1.0/self.f # 程序周期
        self.print_h = 0.5  # 打印周期
        self.vel_thres_min = -0.5    # 速度最低限幅2.0
        self.vel_thres_max = 0.5    # 速度最高限幅2.0
        self.acc_thres_min = -0.1    # 加速度最低限幅2.0
        self.acc_thres_max = 0.1    # 加速度最高限幅2.0
        ############# 状态 初始化 #############
        self.M = 2      # 领导者飞机数量
        self.N = 5      # 跟随者数量
        self.n = np.zeros(self.M+self.N, dtype=int)    # 状态维数
        self.m = np.zeros(self.M+self.N, dtype=int)    # 控制器维数
        self.p = 2      # 输出维数

        ############# 参数 设置 ############## 
        # 系统矩阵  
        self.uav_pos = np.zeros((self.M)*3)
        self.uav_mavPos = np.zeros((self.M)*3)
        self.uav_yaw = np.zeros(self.M)
        self.uav_vel = np.zeros((self.M)*3)
        self.ugv_pos = np.zeros((self.N)*2)
        self.ugv_yaw = np.zeros(self.N)
        self.ugv_vel = np.zeros((self.N)*2)

        # self.t = 0.0  # 时间
        self.command = 0

        # 初始位置
        # self.uav_set_pose = np.append(self.vL_init[0:2], self.xF_init[0:2])
        self.uav_set_pose = np.zeros((self.M)*2)
        # self.ugv_set_pose = self.xF_init[4:]
        self.ugv_set_pose = np.zeros((self.N)*2)

        ############# 参数设置 #############
        self.vel_Px = 1.4
        self.vel_Py = 1.4
        self.vel_Dx = 0.2
        self.vel_Dy = 0.2
        self.w_P = 0.5

        self.vel_Px2 = 1.5
        self.vel_Py2 = 1.5
        self.vel_Dx2 = 0.2
        self.vel_Dy2 = 0.2

        ############# slf变量 #############
        mat_data = loadmat('/home/zrq/Project/exp_for_slf/src/slf_exp/scripts/data_OMAS.mat')
        self.OMAS_L = mat_data['OMAS_L'][0]
        self.AGENT = mat_data['AGENT']
        self.AGENT_A = mat_data['AGENT_A']
        self.AGENT_X = mat_data['AGENT_X']
        self.AGENT_V = mat_data['AGENT_V']

        # 算法参数设定
        self.bar_M = 1  # 动态恶意攻击数量上界
        self.tau1 = 0.45
        self.tau2 = 0.34
        self.alpha = lambda k: (k + 2)**(-self.tau1)
        self.lambda_ = lambda k: 6.9 * (k + 2)**(self.tau1 - self.tau2)  # 避免关键字冲突
        self.umax_1 = 1  # 无人车控制输入限幅
        self.umax_2 = 1  # 无人机控制输入限幅
        self.vmax_2 = 0.5
        self.B = 30  # 二阶系统控制增益

        self.T=40
        self.total_steps = int(self.T/self.dt)-2
        self.num_agents = len(self.OMAS_L)
        self.current_step = 0

        self.flag_ugv1 = 0

        # self.time = 0
        # self.whole_time = 3500
        # self.hgt = 1.0
        self.time_init = 0
        # self.i = 0
        self.uav1_pose_t = np.zeros(2)
        self.uav2_pose_t = np.zeros(2)
        # self.uav3_pose_t = np.zeros(2)
        self.ugv1_pose_t = np.zeros(2)
        self.ugv2_pose_t = np.zeros(2)
        self.ugv3_pose_t = np.zeros(2)
        self.ugv4_pose_t = np.zeros(2)
        self.ugv5_pose_t = np.zeros(2)

        ############# ros 初始化 #############
        rospy.init_node('zrq_control', anonymous=True)
        self.ugv1_vel_pub = rospy.Publisher('/ugv1/cmd_vel', Twist, queue_size=1)
        self.ugv2_vel_pub = rospy.Publisher('/ugv2/cmd_vel', Twist, queue_size=1)
        self.ugv3_vel_pub = rospy.Publisher('/ugv3/cmd_vel', Twist, queue_size=1)
        self.ugv4_vel_pub = rospy.Publisher('/ugv4/cmd_vel', Twist, queue_size=1)
        self.ugv5_vel_pub = rospy.Publisher('/ugv5/cmd_vel', Twist, queue_size=1)
        
        rospy.Subscriber('/command', Int32, self.command_callback)
        rospy.Subscriber('/uav1/pose', PoseStamped, self.uav1_pose_callback, queue_size=1)
        rospy.Subscriber('/uav2/pose', PoseStamped, self.uav2_pose_callback, queue_size=1)
        # rospy.Subscriber('/uav3/pose', PoseStamped, self.uav3_pose_callback, queue_size=1)
        rospy.Subscriber('/ugv1/pose', PoseStamped, self.ugv1_pose_callback, queue_size=1)
        rospy.Subscriber('/ugv2/pose', PoseStamped, self.ugv2_pose_callback, queue_size=1)
        rospy.Subscriber('/ugv3/pose', PoseStamped, self.ugv3_pose_callback, queue_size=1)
        rospy.Subscriber('/ugv4/pose', PoseStamped, self.ugv4_pose_callback, queue_size=1)
        rospy.Subscriber('/ugv5/pose', PoseStamped, self.ugv5_pose_callback, queue_size=1)
        rospy.Subscriber('/uav1/mavros/local_position/velocity_local', TwistStamped, self.uav1_vel_callback, queue_size=1)
        rospy.Subscriber('/uav2/mavros/local_position/velocity_local', TwistStamped, self.uav2_vel_callback, queue_size=1)
        # rospy.Subscriber('/uav3/mavros/local_position/velocity_local', TwistStamped, self.uav3_vel_callback, queue_size=1)
        # rospy.Subscriber('/uav1/twist', TwistStamped, self.uav1_vel_callback, queue_size=1)
        # rospy.Subscriber('/uav2/twist', TwistStamped, self.uav2_vel_callback, queue_size=1)
        rospy.Subscriber('/ugv1/twist', TwistStamped, self.ugv1_vel_callback, queue_size=1)
        rospy.Subscriber('/ugv2/twist', TwistStamped, self.ugv2_vel_callback, queue_size=1)
        rospy.Subscriber('/ugv3/twist', TwistStamped, self.ugv3_vel_callback, queue_size=1)
        rospy.Subscriber('/ugv4/twist', TwistStamped, self.ugv4_vel_callback, queue_size=1)
        rospy.Subscriber('/ugv5/twist', TwistStamped, self.ugv5_vel_callback, queue_size=1)

        ########### minidrone #############
        self.px4_state_ = [State, State, State]
        rospy.Subscriber('/uav1/mavros/state', State, self.uav1_px4state_cb, queue_size=1)
        rospy.Subscriber('/uav2/mavros/state', State, self.uav2_px4state_cb, queue_size=1)
        # rospy.Subscriber('/uav3/mavros/state', State, self.uav3_px4state_cb, queue_size=1)
        self.uav1_armService = rospy.ServiceProxy('/uav1/mavros/cmd/arming', CommandBool)
        self.uav1_flightModeService = rospy.ServiceProxy('/uav1/mavros/set_mode', SetMode)
        self.uav1_target_pose_pub_ = rospy.Publisher('/uav1/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
        self.uav2_armService = rospy.ServiceProxy('/uav2/mavros/cmd/arming', CommandBool)
        self.uav2_flightModeService = rospy.ServiceProxy('/uav2/mavros/set_mode', SetMode)
        self.uav2_target_pose_pub_ = rospy.Publisher('/uav2/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
        # self.uav3_armService = rospy.ServiceProxy('/uav3/mavros/cmd/arming', CommandBool)
        # self.uav3_flightModeService = rospy.ServiceProxy('/uav3/mavros/set_mode', SetMode)
        # self.uav3_target_pose_pub_ = rospy.Publisher('/uav3/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
        rospy.Subscriber("/uav1/mavros/local_position/pose", PoseStamped, self.uav1_mavpose_cb, queue_size=1)
        rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, self.uav2_mavpose_cb, queue_size=1)
        # rospy.Subscriber("/uav3/mavros/local_position/pose", PoseStamped, self.uav3_mavpose_cb, queue_size=1)
        ########### minidrone #############
        self.auto_arm_ = True   # 自动解锁
        self.fly_alt_ = [0.9, 1.1, 1.0]      # 起飞高度
        # self.T_finish_ = 10.0

    def main(self):
        rate = rospy.Rate(self.f)
        step = 0
        last_print_t = 0.0
        init_time = rospy.get_time()
        num_agents = len(self.OMAS_L)
        self.state = 'UGV_STOP'

        self.AGENT_X=self.flatten_nested_arrays(self.AGENT_X)
        self.AGENT_V=self.flatten_nested_arrays(self.AGENT_V)

        AGENT_U = self.create_agent_array((num_agents, self.total_steps+2))
        AGENT_Xhat = self.create_agent_array((num_agents, self.total_steps+2))
        AGENT_Xideal = self.create_agent_array((num_agents, self.total_steps+2))
        AGENT_Xpredict = self.create_agent_array((num_agents, self.total_steps+2))

        # 初始化智能体状态
        for i in range(num_agents):
            agent = self.OMAS_L[i]
            joining_step = agent['joining_step'][0,0]  # MATLAB索引转Python索引
            
            # 加入时刻的初始化
            AGENT_Xhat[i][joining_step] = self.AGENT_X[i][joining_step].copy()
            AGENT_Xpredict[i][joining_step] = self.AGENT_X[i][joining_step].copy()
            
            # 处理不同动力学模型
            dynamics = agent['dynamics'][0,0]
            
            if dynamics == 1:  # 一阶系统
                # 控制输入初始化
                AGENT_U[i][joining_step] = np.array([0.0, 0.0])
                
                # 下一时刻状态初始化
                next_step = joining_step + 1
                if next_step < self.total_steps:
                    AGENT_Xhat[i][next_step] = self.AGENT_X[i][joining_step].copy()
                    AGENT_Xpredict[i][next_step] = self.AGENT_X[i][joining_step].copy()
            
            elif dynamics == 2:  # 二阶系统
                next_step = joining_step + 1
                if next_step < self.total_steps:
                    # 计算初始速度状态
                    velocity = self.AGENT_V[i][joining_step] if joining_step < len(self.AGENT_V[i]) else np.zeros(2)
                    # print(velocity)
                    # 计算下一时刻状态
                    next_state = self.AGENT_X[i][joining_step] + self.dt * velocity
                    
                    AGENT_Xhat[i][next_step] = next_state.copy()
                    AGENT_Xpredict[i][next_step] = next_state.copy()

        # 验证初始化结果
        print(f"初始化完成，共{num_agents}个智能体")
        print(f"时间总步数: {self.total_steps} (T={self.T}, dt={self.dt})")
        print("示例智能体初始状态:")
        print(f"AGENT_Xhat[0] = {AGENT_Xhat[1][1]}")
        print(f"AGENT_Xpredict[0] = {AGENT_Xpredict[1][1]}")
        print(f"AGENT_U[0][0] = {AGENT_U[2][0]}")

        # Main while loop.
        while not rospy.is_shutdown():
            t_loop = rospy.get_time() # time of each loop

            if self.state == 'UGV_STOP':
                self.stop()
                step = 0
                time.sleep(0.2)
                # minidrone
                uav1_mavPE_init_ = self.uav_mavPos[0]
                uav1_mavPN_init_ = self.uav_mavPos[1]
                uav2_mavPE_init_ = self.uav_mavPos[3]
                uav2_mavPN_init_ = self.uav_mavPos[4]
                # uav3_mavPE_init_ = self.uav_mavPos[6]
                # uav3_mavPN_init_ = self.uav_mavPos[7]
                self.time_init_ = rospy.Time.now().to_sec()

            elif self.state == 'TAKEOFF':    # minidrone 起飞
                self.stop() # 无人车停止
                print('take off!')
                # 解锁，auto_arm设置是否可以自动解锁
                if((not self.px4_state_[0].armed) and self.auto_arm_):  #仿真中可以自动解锁
                    if((not self.px4_state_[1].armed) and self.auto_arm_):
                        # if((not self.px4_state_[2].armed) and self.auto_arm_):  #仿真中可以自动解锁
                        self.uav1_arm()
                        self.uav2_arm()
                        # self.uav3_arm()
                        time.sleep(0.5)
                        self.state = 'TAKEOFF_2'
                if(self.px4_state_[0].armed and self.px4_state_[1].armed):  
                    self.state = 'TAKEOFF_2'

            elif self.state == 'TAKEOFF_2':    # minidrone 起飞
                self.stop() # 无人车停止
                print('uav起飞')
                uav1_mavPE_init_ = self.uav_mavPos[0]
                uav1_mavPN_init_ = self.uav_mavPos[1]
                uav2_mavPE_init_ = self.uav_mavPos[3]
                uav2_mavPN_init_ = self.uav_mavPos[4]
                # uav3_mavPE_init_ = self.uav_mavPos[6]
                # uav3_mavPN_init_ = self.uav_mavPos[7]

                ugv1_mavPE_init_ = self.ugv_pos[0]
                ugv1_mavPN_init_ = self.ugv_pos[1]
                ugv2_mavPE_init_ = self.ugv_pos[2]
                ugv2_mavPN_init_ = self.ugv_pos[3]
                ugv3_mavPE_init_ = self.ugv_pos[4]
                ugv3_mavPN_init_ = self.ugv_pos[5]
                ugv4_mavPE_init_ = self.ugv_pos[6]
                ugv4_mavPN_init_ = self.ugv_pos[7]
                ugv5_mavPE_init_ = self.ugv_pos[8]
                ugv5_mavPN_init_ = self.ugv_pos[9]

                uav1_PxyPz_sp = [uav1_mavPE_init_, uav1_mavPN_init_, self.fly_alt_[0]]
                self.pubPxyPzCmd(uav1_PxyPz_sp, 1)
                uav2_PxyPz_sp = [uav2_mavPE_init_, uav2_mavPN_init_, self.fly_alt_[1]]
                self.pubPxyPzCmd(uav2_PxyPz_sp, 2)
                # uav3_PxyPz_sp = [uav3_mavPE_init_, uav3_mavPN_init_, self.fly_alt_[2]]
                # self.pubPxyPzCmd(uav3_PxyPz_sp, 3)
                time.sleep(0.5)
                if abs(self.fly_alt_[0] - self.uav_mavPos[2]) < 0.1 and abs(self.uav_set_pose[0] - self.uav_mavPos[0]) < 0.1 and abs(self.uav_set_pose[1] - self.uav_mavPos[1]) < 0.1:
                    if abs(self.fly_alt_[1] - self.uav_mavPos[5]) < 0.1 and abs(self.uav_set_pose[2] - self.uav_mavPos[3]) < 0.1 and abs(self.uav_set_pose[3] - self.uav_mavPos[4]) < 0.1:
                        # if abs(self.fly_alt_[2] - self.uav_mavPos[8]) < 0.1 and abs(self.uav_set_pose[4] - self.uav_mavPos[6]) < 0.1 and abs(self.uav_set_pose[5] - self.uav_mavPos[7]) < 0.1:
                        print('已完成起飞!')
                        self.state = 'UAV_UGV_STOP'    # 停止
                self.time_init = rospy.Time.now().to_sec()

            elif self.state == 'UAV_UGV_STOP':
                self.stop() # 无人车停止
                # minidrone 记录当前坐标并停止
                uav1_mavPE_init_ = self.uav_mavPos[0]
                uav1_mavPN_init_ = self.uav_mavPos[1]
                uav2_mavPE_init_ = self.uav_mavPos[3]
                uav2_mavPN_init_ = self.uav_mavPos[4]
                # uav3_mavPE_init_ = self.uav_mavPos[6]
                # uav3_mavPN_init_ = self.uav_mavPos[7]
                # 保持无人机静止，等待降落或其他任务
                uav1_PxyPz_sp = [uav1_mavPE_init_, uav1_mavPN_init_, self.fly_alt_[0]]
                uav2_PxyPz_sp = [uav2_mavPE_init_, uav2_mavPN_init_, self.fly_alt_[1]]
                # uav3_PxyPz_sp = [uav3_mavPE_init_, uav3_mavPN_init_, self.fly_alt_[2]]
                self.pubPxyPzCmd(uav1_PxyPz_sp, 1)
                self.pubPxyPzCmd(uav2_PxyPz_sp, 2)
                # self.pubPxyPzCmd(uav3_PxyPz_sp, 3)
                self.state = 'UAV_UGV_STOP_2'
                self.time_init = rospy.Time.now().to_sec()

            elif self.state == 'UAV_UGV_STOP_2':
                # 无人机-无人车持续停在当前位置
                self.stop() # 无人车停止
                # 保持无人机静止，等待降落或其他任务
                uav1_PxyPz_sp = [uav1_mavPE_init_, uav1_mavPN_init_, self.fly_alt_[0]]
                uav2_PxyPz_sp = [uav2_mavPE_init_, uav2_mavPN_init_, self.fly_alt_[1]]
                # uav3_PxyPz_sp = [uav3_mavPE_init_, uav3_mavPN_init_, self.fly_alt_[2]]
                self.pubPxyPzCmd(uav1_PxyPz_sp, 1)
                self.pubPxyPzCmd(uav2_PxyPz_sp, 2)
                # self.pubPxyPzCmd(uav3_PxyPz_sp, 3)
                self.time_init = rospy.Time.now().to_sec()


            elif self.state == 'TEST':
                # 测试状态机
                # 持续发当前位置指令
                self.pubPxyPzCmd(uav1_PxyPz_sp, 1)
                self.pubPxyPzCmd(uav2_PxyPz_sp, 2)
                # self.pubPxyPzCmd(uav3_PxyPz_sp, 3)
                if rospy.Time.now().to_sec() - self.time_init_ > 15.0:
                    self.state = 'LAND'

            elif self.state == 'LAND':
                self.stop() # 无人车停止
                print('收到降落指令')
                if (self.px4_state_[0].mode != "OFFBOARD") or (self.px4_state_[1].mode != "OFFBOARD"):
                    print('等待offboard!')
                else:
                    # time_finish_ = rospy.Time.now().to_sec() - time_init_
                    uav1_mavPE_init_ = self.uav_mavPos[0]
                    uav1_mavPN_init_ = self.uav_mavPos[1]
                    uav1_mavPZ_init_ = self.uav_mavPos[2]
                    uav2_mavPE_init_ = self.uav_mavPos[3]
                    uav2_mavPN_init_ = self.uav_mavPos[4]
                    uav2_mavPZ_init_ = self.uav_mavPos[5]
                    # uav3_mavPE_init_ = self.uav_mavPos[6]
                    # uav3_mavPN_init_ = self.uav_mavPos[7]
                    # uav3_mavPZ_init_ = self.uav_mavPos[8]
                    self.state = 'LAND_2'

            elif self.state == 'LAND_2':
                self.stop() # 无人车停止
                print('降落!')
                # t_finish_= rospy.Time.now().to_sec() - time_finish_
                # if (t_finish_ >= self.T_finish_):
                #     t_finish_ = self.T_finish_
                
                uav1_PxyPz_sp = [uav1_mavPE_init_, uav1_mavPN_init_, uav1_mavPZ_init_]
                uav2_PxyPz_sp = [uav2_mavPE_init_, uav2_mavPN_init_, uav2_mavPZ_init_]
                # uav3_PxyPz_sp = [uav3_mavPE_init_, uav3_mavPN_init_, uav3_mavPZ_init_]
                # uav1
                if(self.uav_mavPos[2] > 0.25):
                    uav1_VxyVz_sp = [0.0, 0.0, -0.2]
                    self.pubPxyVzCmd(uav1_PxyPz_sp, uav1_VxyVz_sp, 1)
                else:
                    # PxyPz_sp << hover_x, hover_y, 0.2
                    uav1_VxyVz_sp = [0.0, 0.0, -0.8]
                    self.pubPxyVzCmd(uav1_PxyPz_sp, uav1_VxyVz_sp, 1)
                # uav2
                if(self.uav_mavPos[5] > 0.25):
                    uav2_VxyVz_sp = [0.0, 0.0, -0.2]
                    self.pubPxyVzCmd(uav2_PxyPz_sp, uav2_VxyVz_sp, 2)
                else:
                    uav2_VxyVz_sp = [0.0, 0.0, -0.8]
                    self.pubPxyVzCmd(uav2_PxyPz_sp, uav2_VxyVz_sp, 2)
                # uav3
                # if(self.uav_mavPos[8] > 0.25):
                #     uav3_VxyVz_sp = [0.0, 0.0, -0.2]
                #     self.pubPxyVzCmd(uav3_PxyPz_sp, uav3_VxyVz_sp, 3)
                # else:
                #     uav3_VxyVz_sp = [0.0, 0.0, -0.8]
                #     self.pubPxyVzCmd(uav3_PxyPz_sp, uav3_VxyVz_sp, 3)

            elif self.state == 'RESET':
                step = 0
                # print(self.ugv_vel)
                ugv1_vel_cmd = self.vel_control(self.ugv_pos[0:2], self.ugv_set_pose[0:2], self.ugv_yaw[0], 0.0, 'ugv')
                ugv2_vel_cmd = self.vel_control(self.ugv_pos[2:4], self.ugv_set_pose[2:4], self.ugv_yaw[1], 0.0, 'ugv')
                ugv3_vel_cmd = self.vel_control(self.ugv_pos[4:6], self.ugv_set_pose[4:6], self.ugv_yaw[2], 0.0, 'ugv')
                # ugv4_vel_cmd = self.vel_control(self.ugv_pos[8:10], self.ugv_set_pose[8:10], self.ugv_yaw[4], 0.0, 'ugv')
                uav1_vel_cmd = self.vel_control(self.uav_pos[0:2], self.uav_set_pose[0:2], self.uav_yaw[0], 0.0, 'uav')
                uav2_vel_cmd = self.vel_control(self.uav_pos[3:5], self.uav_set_pose[2:4], self.uav_yaw[1], 0.0, 'uav')
                # uav3_vel_cmd = self.vel_control(self.uav_pos[6:8], self.uav_set_pose[4:6], self.uav_yaw[2], 0.0, 'ugv')
                self.ugv1_vel_pub.publish(ugv1_vel_cmd)
                self.ugv2_vel_pub.publish(ugv2_vel_cmd)
                self.ugv3_vel_pub.publish(ugv3_vel_cmd)
                self.ugv4_vel_pub.publish(ugv4_vel_cmd)
                self.ugv5_vel_pub.publish(ugv5_vel_cmd)
                # mini-drone 
                uav1_VxyPz_sp = [uav1_vel_cmd.linear.x, uav1_vel_cmd.linear.y, self.fly_alt_[0]]
                uav1_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav1_VxyPz_sp, uav1_yaw_sp_, 1)
                uav2_VxyPz_sp = [uav2_vel_cmd.linear.x, uav2_vel_cmd.linear.y, self.fly_alt_[1]]
                uav2_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav2_VxyPz_sp, uav2_yaw_sp_, 2)
                # uav3_VxyPz_sp = [uav3_vel_cmd.linear.x, uav3_vel_cmd.linear.y, self.fly_alt_[2]]
                # uav3_yaw_sp_ = 0.0
                # self.pubVxyPzYawCmd(uav3_VxyPz_sp, uav3_yaw_sp_, 3)

            elif self.state == 'PREPARE':
                
                Times = rospy.Time.now().to_sec()-self.time_init
                self.uav1_pose_prpx = uav1_mavPE_init_
                self.uav1_pose_prpy = uav1_mavPN_init_
                self.uav2_pose_prpx = uav2_mavPE_init_
                self.uav2_pose_prpy = uav2_mavPN_init_
                # self.uav3_pose_prpx = uav3_mavPE_init_
                # self.uav3_pose_prpy = uav3_mavPN_init_

                self.ugv1_pose_prpx = ugv1_mavPE_init_
                self.ugv1_pose_prpy = ugv1_mavPN_init_
                self.ugv2_pose_prpx = ugv2_mavPE_init_
                self.ugv2_pose_prpy = ugv2_mavPN_init_
                self.ugv3_pose_prpx = ugv3_mavPE_init_
                self.ugv3_pose_prpy = ugv3_mavPN_init_
                self.ugv4_pose_prpx = ugv4_mavPE_init_
                self.ugv4_pose_prpy = ugv4_mavPN_init_
                self.ugv5_pose_prpx = ugv5_mavPE_init_
                self.ugv5_pose_prpy = ugv5_mavPN_init_

                # 10秒内到达指定的地点准备执行算法
                if  Times <= 10: 
                    self.uav1_pose_t[0] = (3.66-self.uav1_pose_prpx)/10 * Times + self.uav1_pose_prpx
                    self.uav1_pose_t[1] = (1.97-self.uav1_pose_prpy)/10 * Times + self.uav1_pose_prpy
                    self.uav2_pose_t[0] = (2.96-self.uav2_pose_prpx)/10 * Times + self.uav2_pose_prpx
                    self.uav2_pose_t[1] = (2.67-self.uav2_pose_prpy)/10 * Times + self.uav2_pose_prpy
                    # self.uav3_pose_t[0] = (-5.134-self.uav3_pose_prpx)/10 * Times + self.uav3_pose_prpx
                    # self.uav3_pose_t[1] = (-4.5-self.uav3_pose_prpy)/10 * Times + self.uav3_pose_prpy

                    self.ugv1_pose_t[0] = (2.70-self.ugv1_pose_prpx)/10 * Times + self.ugv1_pose_prpx
                    self.ugv1_pose_t[1] = (2.20-self.ugv1_pose_prpy)/10 * Times + self.ugv1_pose_prpy
                    self.ugv2_pose_t[0] = (2.70-self.ugv2_pose_prpx)/10 * Times + self.ugv2_pose_prpx
                    self.ugv2_pose_t[1] = (1.70-self.ugv2_pose_prpy)/10 * Times + self.ugv2_pose_prpy
                    self.ugv3_pose_t[0] = (3.12-self.ugv3_pose_prpx)/10 * Times + self.ugv3_pose_prpx
                    self.ugv3_pose_t[1] = (1.23-self.ugv3_pose_prpy)/10 * Times + self.ugv3_pose_prpy
                    self.ugv4_pose_t[0] = (4.05-self.ugv4_pose_prpx)/10 * Times + self.ugv4_pose_prpx
                    self.ugv4_pose_t[1] = (0.58-self.ugv4_pose_prpy)/10 * Times + self.ugv4_pose_prpy
                    self.ugv5_pose_t[0] = (4.25-self.ugv5_pose_prpx)/10 * Times + self.ugv5_pose_prpx
                    self.ugv5_pose_t[1] = (0.18-self.ugv5_pose_prpy)/10 * Times + self.ugv5_pose_prpy

                else:
                    self.uav1_pose_t[0] = 3.66
                    self.uav1_pose_t[1] = 1.97
                    self.uav2_pose_t[0] = 2.96
                    self.uav2_pose_t[1] = 2.67
                    # self.uav3_pose_t[0] = (-5.134-self.uav3_pose_prpx)/10 * Times + self.uav3_pose_prpx
                    # self.uav3_pose_t[1] = (-4.5-self.uav3_pose_prpy)/10 * Times + self.uav3_pose_prpy

                    self.ugv1_pose_t[0] = 2.70
                    self.ugv1_pose_t[1] = 2.20
                    self.ugv2_pose_t[0] = 2.70
                    self.ugv2_pose_t[1] = 1.70
                    self.ugv3_pose_t[0] = 3.12
                    self.ugv3_pose_t[1] = 1.23
                    self.ugv4_pose_t[0] = 4.05
                    self.ugv4_pose_t[1] = 0.58
                    self.ugv5_pose_t[0] = 4.25
                    self.ugv5_pose_t[1] = 0.18

                    # self.state = 'ALGRITHM'
                    # print(Times,self.uav1_pose_prpx,self.uav1_pose_prpy)

                # print(Times,self.uav1_pose_prpx,self.uav1_pose_prpy)

                uav1_vel_cmd = self.vel_control(self.uav_pos[0:2], self.uav1_pose_t, self.uav_yaw[0], 0.0, 'uav')
                uav2_vel_cmd = self.vel_control(self.uav_pos[3:5], self.uav2_pose_t, self.uav_yaw[1], 0.0, 'uav')
                # uav3_vel_cmd = self.vel_control(self.uav_pos[6:8], self.uav3_pose_t, self.uav_yaw[2], 0.0, 'uav')

                ugv1_vel_cmd = self.vel_control(self.ugv_pos[0:2], self.ugv1_pose_t, self.ugv_yaw[0], 0.0, 'ugv')
                ugv2_vel_cmd = self.vel_control(self.ugv_pos[2:4], self.ugv2_pose_t, self.ugv_yaw[1], 0.0, 'ugv')
                ugv3_vel_cmd = self.vel_control(self.ugv_pos[4:6], self.ugv3_pose_t, self.ugv_yaw[2], 0.0, 'ugv')
                ugv4_vel_cmd = self.vel_control(self.ugv_pos[6:8], self.ugv4_pose_t, self.ugv_yaw[3], 0.0, 'ugv')
                ugv5_vel_cmd = self.vel_control(self.ugv_pos[8:10], self.ugv5_pose_t, self.ugv_yaw[4], 0.0, 'ugv')

                self.ugv1_vel_pub.publish(ugv1_vel_cmd)
                self.ugv2_vel_pub.publish(ugv2_vel_cmd)
                self.ugv3_vel_pub.publish(ugv3_vel_cmd)
                self.ugv4_vel_pub.publish(ugv4_vel_cmd)
                self.ugv5_vel_pub.publish(ugv5_vel_cmd)

                uav1_VxyPz_sp = [uav1_vel_cmd.linear.x, uav1_vel_cmd.linear.y, self.fly_alt_[0]]
                uav1_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav1_VxyPz_sp, uav1_yaw_sp_, 1)
                uav2_VxyPz_sp = [uav2_vel_cmd.linear.x, uav2_vel_cmd.linear.y, self.fly_alt_[1]]
                uav2_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav2_VxyPz_sp, uav2_yaw_sp_, 2)
                # uav3_VxyPz_sp = [uav3_vel_cmd.linear.x, uav3_vel_cmd.linear.y, self.fly_alt_[1]]
                # uav3_yaw_sp_ = 0.0
                # self.pubVxyPzYawCmd(uav3_VxyPz_sp, uav3_yaw_sp_, 3)             
            
            elif self.state == 'ALGRITHM':
                
                # uav_cmd_pos = np.zeros((self.M)*2)
                # ugv_cmd_pos = np.zeros((self.N)*2)
                k = self.current_step 
                Times = rospy.Time.now().to_sec()-self.time_init

                AGENT_tmp = np.where(self.AGENT[:, k] >= 1)[0].tolist()
                AGENT_N_tmp = np.where(self.AGENT[:, k] == 1)[0].tolist()
                AGENT_M_tmp = np.where(self.AGENT[:, k] == 2)[0].tolist()

                AGENT_h_tmp = {i: None for i in range(len(self.OMAS_L))}
                for i in AGENT_tmp:
                    idx = AGENT_tmp.index(i)
                    AGENT_h_tmp[i] = self.function_hL(idx, len(AGENT_tmp), k, self.dt)
                
                alpha_tmp = self.alpha(k)
                lambda_tmp = self.lambda_(k)

                # 下一时刻数据
                if k != self.total_steps:
                    next_step = k + 1
                    AGENT_next_tmp = np.where(self.AGENT[:, next_step] >= 1)[0].tolist()
                    AGENT_N_next_tmp = np.where(self.AGENT[:, next_step] == 1)[0].tolist()
                    AGENT_M_next_tmp = np.where(self.AGENT[:, next_step] == 2)[0].tolist()
                else:
                    AGENT_next_tmp = AGENT_tmp.copy()
                    AGENT_N_next_tmp = AGENT_N_tmp.copy()
                    AGENT_M_next_tmp = AGENT_M_tmp.copy()

                AGENT_h_next_tmp = {i: None for i in range(len(self.OMAS_L))}
                for i in range(len(self.OMAS_L)):
                    if i in AGENT_next_tmp:
                        idx = AGENT_next_tmp.index(i)
                        AGENT_h_next_tmp[i] = self.function_hL(idx, len(AGENT_next_tmp), k+1, self.dt)
                    elif i in list(set(AGENT_tmp) - set(AGENT_next_tmp)):
                        AGENT_h_next_tmp[i] = AGENT_h_tmp[i]
                
                alpha_next_tmp=self.alpha(k+1)
                lambda_next_tmp=self.lambda_(k+1)

                # 时刻k+2的相关数据
                if k != self.total_steps and k != self.total_steps-1:
                    next_step = k + 2
                    AGENT_next_tmp2 = np.where(self.AGENT[:, next_step] >= 1)[0].tolist()
                else:
                    AGENT_next_tmp2 = AGENT_next_tmp

                AGENT_h_next_tmp2 = {i: None for i in range(self.AGENT.shape[0])} 
                # 各智能体时刻k+1的编队偏移【N*1元组，各元组为2*1向量（第i个元组代表智能体i在时刻k+1的编队偏移）】（注：非时刻k或非时刻k+2参与的智能体对应的元组为空集）
                for i in range(len(self.OMAS_L)):
                    if i in AGENT_next_tmp2:
                        idx = AGENT_next_tmp2.index(i)
                        AGENT_h_next_tmp2[i]=self.function_hL(idx,len(AGENT_next_tmp2),k+2,self.dt)
                    elif i in list(set(AGENT_next_tmp) - set(AGENT_next_tmp2)):
                        AGENT_h_next_tmp2[i]=AGENT_h_next_tmp[i]
                
                # 处理不同阶数的智能体
                # 1阶正常智能体
                for i in AGENT_N_next_tmp:
                    agent = self.OMAS_L[i]
                    if agent['dynamics'][0,0] == 1:
                        # 状态更新和控制逻辑
                        if AGENT_Xhat[i][k] is None:
                            AGENT_Xhat[i][k] = self.AGENT_X[i][k]
                            AGENT_Xhat[i][k+1] = self.AGENT_X[i][k]
                        
                        if k != self.total_steps:
                            # print(AGENT_A,k)
                            neighbors = np.where(self.AGENT_A[k+1][0][i] == 1)[0].tolist()
                        else:
                            neighbors = np.where(self.AGENT_A[k][0][i] == 1)[0].tolist()
                        
                        # 邻居交互处理
                        # AGENT_X_tmp = create_agent_array((num_agents, 1))
                        AGENT_X_tmp = {}
                        invalid_neighbors = []
                        for j in neighbors:
                            if j in AGENT_N_next_tmp:
                                AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                                # print(AGENT_X_tmp,"1111")
                            else:
                                AGENT_X_tmp[j] = self.function_attack(AGENT_Xpredict[i][k+1], 
                                                                AGENT_Xpredict[i][k-65+1], 
                                                                AGENT_Xpredict[j][k+1], i, k+1, self.dt)
                            if len(AGENT_X_tmp[j])==0:
                                invalid_neighbors.append(j)
                        
                        valid_neighbors = [j for j in neighbors if j not in invalid_neighbors]
                        
                        # print(AGENT_X_tmp,neighbors,AGENT_N_next_tmp,"1111")
                        # 状态更新
                        # print(neighbors,AGENT_X_tmp,AGENT_h_next_tmp)
                        AGENT_Xhat[i][k+2] = self.function_update(
                            self.bar_M - len(invalid_neighbors),
                            AGENT_Xpredict[i][k+1],
                            AGENT_X_tmp,
                            valid_neighbors,
                            AGENT_h_next_tmp,
                            alpha_tmp,
                            lambda_tmp,
                            i,
                            k+1,
                            AGENT_h_next_tmp2.get(i),self.dt
                        )
                        # print(AGENT_Xpredict[i][k+1],AGENT_Xhat[i][k+2])
                        # 控制输入计算 目前已完成到这里
                        AGENT_U[i][k+1] = self.function_control_1(
                            AGENT_Xpredict[i][k+1],
                            AGENT_Xhat[i][k+2],
                            self.umax_1,self.dt
                        )
                        # print(AGENT_U[i][k+1])
                        # 状态预测 k还没改
                        AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + self.dt * AGENT_U[i][k+1]
                        # print

                        if i in AGENT_tmp:
                            # 单纯仿真，无闭环实验仿真，用积分更新位置
                            self.AGENT_X[i][k+1], self.AGENT_V[i][k+1] = self.function_trans_1(
                                self.AGENT_X[i][k],
                                AGENT_U[i][k],
                                self.dt
                            )

                        if np.linalg.norm(self.AGENT_X[i][k+1],ord=2) > 100000:
                            print(111111111111111111)

                # 恶意一阶智能体处理
                for i in AGENT_M_next_tmp:
                    agent = self.OMAS_L[i]
                    if agent['dynamics'][0,0] == 1:
                        # 初始化理想状态
                        if len(AGENT_Xideal[i][k])==0:
                            AGENT_Xideal[i][k] = self.AGENT_X[i][k].copy()
                            AGENT_Xideal[i][k+1] = self.AGENT_X[i][k].copy()
                        
                        # 获取邻居信息
                        if k != self.total_steps:
                            neighbors = np.where(self.AGENT_A[k+1][0][i] == 1)[0].tolist()
                        else:
                            neighbors = np.where(self.AGENT_A[k][0][i] == 1)[0].tolist()
                        
                        # 排除自身后的邻居集合
                        filtered_neighbors = [j for j in neighbors if j != i]
                        
                        # 收集邻居信息
                        AGENT_X_tmp = {}
                        invalid_neighbors = []
                        for j in filtered_neighbors:
                            if j in AGENT_N_next_tmp:
                                AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                            else:
                                AGENT_X_tmp[j] = self.function_attack(
                                    AGENT_Xpredict[i][k+1],
                                    AGENT_Xpredict[i][k-65+1],  # 修正索引偏移: k-65+2 = k-63
                                    AGENT_Xpredict[j][k+1],
                                    i, k+1, self.dt
                                )
                            if len(AGENT_X_tmp[j])==0:
                                invalid_neighbors.append(j)
                        
                        # 更新有效邻居列表
                        valid_neighbors = [j for j in filtered_neighbors if j not in invalid_neighbors]
                        
                        # print(AGENT_X_tmp,"2222")
                        # 状态更新（恶意版本）
                        xhat_ideal = self.function_update_malicious(
                            self.bar_M - len(invalid_neighbors),
                            AGENT_Xideal[i][k+1],
                            AGENT_X_tmp,
                            valid_neighbors,
                            AGENT_h_next_tmp,
                            i,
                            AGENT_h_next_tmp2.get(i)
                        )
                        
                        # 理想控制量计算
                        u_tmp_ideal = self.function_control_1(
                            AGENT_Xideal[i][k+1],
                            xhat_ideal,
                            self.umax_1,self.dt
                        )
                        
                        # 更新理想状态
                        AGENT_Xideal[i][k+2] = AGENT_Xideal[i][k+1] + self.dt * u_tmp_ideal
                        
                        # 控制增益计算
                        if k * self.dt > 10:
                            u_gain = (1 + 0.05 * np.sin((k+1)*self.dt)) * (1 - 0.1 * np.exp(-(k+1)*self.dt))
                        else:
                            u_gain = 1.0
                        
                        # 实际控制量应用
                        AGENT_U[i][k+1] = u_gain * u_tmp_ideal
                        
                        # 状态预测
                        AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + self.dt * AGENT_U[i][k+1]
                        
                        # 实际状态转移
                        self.AGENT_X[i][k+1], self.AGENT_V[i][k+1] = self.function_trans_1(
                            self.AGENT_X[i][k],
                            AGENT_U[i][k],
                            self.dt
                        )

                        if np.linalg.norm(self.AGENT_X[i][k+1],ord=2) > 100000:
                            print(111111111111111111)

                # 2阶正常智能体
                for i in AGENT_N_next_tmp:
                    agent = self.OMAS_L[i]
                    if agent['dynamics'][0,0] == 2:
                        
                        if k != self.total_steps:
                            neighbors = np.where(self.AGENT_A[k+1][0][i] == 1)[0].tolist()
                        else:
                            neighbors = np.where(self.AGENT_A[k][0][i] == 1)[0].tolist()
                        
                        # 邻居交互处理
                        AGENT_X_tmp = {}
                        invalid_neighbors = []
                        for j in neighbors:
                            if j in AGENT_N_next_tmp:
                                AGENT_X_tmp[j] = AGENT_Xpredict[j][k+1]
                                # print(AGENT_X_tmp,"2222")
                            else:
                                AGENT_X_tmp[j] = self.function_attack(AGENT_Xpredict[i][k+1], 
                                                                AGENT_Xpredict[i][k-65+1], 
                                                                AGENT_Xpredict[j][k+1], i, k+1, self.dt)
                            if len(AGENT_X_tmp[j])==0:
                                invalid_neighbors.append(j)
                        
                        valid_neighbors = [j for j in neighbors if j not in invalid_neighbors]
                        # print(AGENT_X_tmp,"3333")
                        # 状态更新
                        AGENT_Xhat[i][k+2] = self.function_update(
                            self.bar_M - len(invalid_neighbors),
                            AGENT_Xpredict[i][k+1],
                            AGENT_X_tmp,
                            valid_neighbors,
                            AGENT_h_next_tmp,
                            alpha_tmp,
                            lambda_tmp,
                            i,
                            k+1,
                            AGENT_h_next_tmp2.get(i),self.dt
                        )
                        # print(AGENT_Xhat[i][k+2])
                        # 控制输入计算
                        AGENT_U[i][k] = self.function_control_2(
                            self.AGENT_X[i][k],
                            self.AGENT_V[i][k],
                            AGENT_Xpredict[i][k+1],
                            AGENT_Xhat[i][k+2],
                            self.umax_2,
                            self.vmax_2,self.B,self.dt
                        )

                        # print(AGENT_U[i][k])

                        # 时刻k+2的预计算状态
                        v_tmp=self.AGENT_V[i][k]+self.B*AGENT_U[i][k]*self.dt
                        # 状态预测
                        AGENT_Xpredict[i][k+2] = AGENT_Xpredict[i][k+1] + self.dt * v_tmp
                        
                        if (i in AGENT_N_tmp) and (i in AGENT_N_next_tmp) :
                            self.AGENT_X[i][k+1], self.AGENT_V[i][k+1] = self.function_trans_2(
                                self.AGENT_X[i][k],
                                self.AGENT_V[i][k],
                                self.B*AGENT_U[i][k],
                                self.dt
                            )
                        
                        if np.linalg.norm(self.AGENT_X[i][k+1],ord=2) > 100000:
                            print(111111111111111111)
                
                uav1_vel_cmd = self.vel_control_PD2(self.uav_pos[0:2], self.AGENT_X[0][k], self.uav_vel[0:2], np.clip(self.AGENT_V[0][k], -0.5, 0.5),self.uav_yaw[0], 0.0, 'uav')
                uav2_vel_cmd = self.vel_control_PD2(self.uav_pos[3:5], self.AGENT_X[1][k], self.uav_vel[3:5], np.clip(self.AGENT_V[1][k], -0.5, 0.5),self.uav_yaw[0], 0.0, 'uav')
    
                uav1_VxyPz_sp = [uav1_vel_cmd.linear.x, uav1_vel_cmd.linear.y, self.fly_alt_[0]]
                uav1_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav1_VxyPz_sp, uav1_yaw_sp_, 1)
                uav2_VxyPz_sp = [uav2_vel_cmd.linear.x, uav2_vel_cmd.linear.y, self.fly_alt_[1]]
                uav2_yaw_sp_ = 0.0
                self.pubVxyPzYawCmd(uav2_VxyPz_sp, uav2_yaw_sp_, 2)
                
                # print(AGENT_U[4][k][0])
                if len(AGENT_U[2][k-1])!=0 and len(AGENT_U[2][k])==0:
                    self.flag_ugv1 = 1

                if self.flag_ugv1 == 1:
                    ugv1_vel_cmd = self.vel_control(self.ugv_pos[0:2], [4.2,2.1], self.ugv_yaw[0], 0.0, 'ugv')
                else:
                    ugv1_vel_cmd = self.vel_control_PD(self.ugv_pos[0:2], self.AGENT_X[2][k], self.ugv_vel[0:2], np.clip(AGENT_U[2][k], -0.5, 0.5),self.ugv_yaw[0], 0.0, 'ugv')

                ugv2_vel_cmd = self.vel_control_PD(self.ugv_pos[2:4], self.AGENT_X[3][k], self.ugv_vel[2:4], np.clip(AGENT_U[3][k], -0.5, 0.5),self.ugv_yaw[1], 0.0, 'ugv')
                ugv3_vel_cmd = self.vel_control_PD(self.ugv_pos[4:6], self.AGENT_X[4][k], self.ugv_vel[4:6], np.clip(AGENT_U[4][k], -0.5, 0.5),self.ugv_yaw[2], 0.0, 'ugv')
                ugv4_vel_cmd = self.vel_control_PD(self.ugv_pos[6:8], self.AGENT_X[5][k], self.ugv_vel[6:8], np.clip(AGENT_U[5][k], -0.5, 0.5),self.ugv_yaw[3], 0.0, 'ugv')
                ugv5_vel_cmd = self.vel_control_PD(self.ugv_pos[8:10], self.AGENT_X[6][k], self.ugv_vel[8:10], np.clip(AGENT_U[6][k], -0.5, 0.5),self.ugv_yaw[4], 0.0, 'ugv')
                self.ugv1_vel_pub.publish(ugv1_vel_cmd)
                self.ugv2_vel_pub.publish(ugv2_vel_cmd)
                self.ugv3_vel_pub.publish(ugv3_vel_cmd)
                self.ugv4_vel_pub.publish(ugv4_vel_cmd)
                self.ugv5_vel_pub.publish(ugv5_vel_cmd)

                self.current_step += 1

                if (self.uav_pos[0] <-2 or self.uav_pos[0] >8) and (self.uav_pos[1] <-2 or self.uav_pos[1] >7)\
                    and (self.uav_pos[3] <-2 or self.uav_pos[3] >8) and (self.uav_pos[4] <-2 or self.uav_pos[4] >7)\
                    and (self.ugv_pos[0] <-2 or self.ugv_pos[0] >8) and (self.ugv_pos[1] <-2 or self.ugv_pos[1] >7)\
                    and (self.ugv_pos[2] <-2 or self.ugv_pos[2] >8) and (self.ugv_pos[3] <-2 or self.ugv_pos[3] >7)\
                    and (self.ugv_pos[4] <-2 or self.ugv_pos[4] >8) and (self.ugv_pos[5] <-2 or self.ugv_pos[5] >7)\
                    and (self.ugv_pos[6] <-2 or self.ugv_pos[6] >8) and (self.ugv_pos[7] <-2 or self.ugv_pos[7] >7)\
                    and (self.ugv_pos[8] <-2 or self.ugv_pos[8] >8) and (self.ugv_pos[9] <-2 or self.ugv_pos[9] >7):
                    self.state = 'UAV_UGV_STOP'

                if self.current_step >= 3970:

                    uav1_VxyPz_sp = [0, 0, self.fly_alt_[0]]
                    uav1_yaw_sp_ = 0.0
                    self.pubVxyPzYawCmd(uav1_VxyPz_sp, uav1_yaw_sp_, 1)
                    uav2_VxyPz_sp = [0, 0, self.fly_alt_[1]]
                    uav2_yaw_sp_ = 0.0
                    self.pubVxyPzYawCmd(uav2_VxyPz_sp, uav2_yaw_sp_, 2)

                if self.current_step == 3998:
                    self.state = 'UAV_UGV_STOP'
            
            rate.sleep()
    
    # 加载MATLAB数据
    def flatten_nested_arrays(self,data):
        array_m=[]
        for data1 in data:
            processed = []
            for item in data1:
                # 检查是否为NumPy数组且非空
                if isinstance(item, np.ndarray) and item.size > 0:
                    # 若数组是二维或更高维，则扁平化
                    if item.ndim >= 2:
                        flattened = item.flatten()
                        # print(flattened)
                        processed.append(flattened)
                        # processed.append(flattened.tolist())  # 转换为列表
                    else:
                        processed.append(np.array(item.tolist()))
                # 空数组或非数组元素保持原样
                else:
                    processed.append(item)
            array_m.append(processed)
        return array_m

    # 初始化数据结构
    def create_agent_array(self,shape):
        """创建智能体状态存储数组"""
        return [[[] for _ in range(shape[1])] for _ in range(shape[0])]

    def function_gL(self, i, x, k):
        """本地目标函数g实现"""
        g=0.05*np.linalg.norm(x,ord=2)
        return g

    def function_hL(self, i, num_AGENT, k, dt):
        """
        编队偏移向量计算函数
        
        参数:
        i : int - 智能体索引
        num_AGENT : int - 当前参与的智能体总数
        k : int - 当前时间步索引
        dt : float - 时间间隔
        
        返回:
        numpy.ndarray - 二维偏移向量 [2,]
        """
        angle = 2 * np.pi * i / num_AGENT + np.pi * k * dt / 80
        h_L = 0.7 * np.array([np.cos(angle), np.sin(angle)])
        return h_L

    def function_attack(self, xi,xi_past,xj,i,k,dt):
        if k*dt<=9:
            # print(xj,xi)
            # print(1111)
            x=xj+math.sin((i+1)*k*dt)*np.max(np.vstack((xj,xi)),axis=0) # FDI（错误数据注入）攻击
        elif k*dt>9 and k*dt<=25:
            x=[] # DOS（拒绝服务）攻击
        elif k*dt>25 and k*dt<=26:
            x=xj+6.8*math.cos(k*dt)*np.ones((1,2))[0] # FDI（错误数据注入）攻击
            # print(6.8*math.cos(k*dt)*np.ones((1,2))[0])
            # print(3333)
        else:
            # print(4444)
            x=xi_past # 重放攻击
        # print(x)
        return x

    def function_update(self, bar_M, x_tmp, AGENT_X_tmp, Neighbor_tmp, AGENT_h_tmp, alpha_tmp, lambda_tmp, i, k, h_next_tmp, dt):
        """
        估计值更新函数
        
        参数:
        bar_M -- 弹性邻居数量
        x_tmp -- 当前状态估计
        AGENT_X_tmp -- 邻居状态信息字典
        Neighbor_tmp -- 有效邻居索引列表
        AGENT_h_tmp -- 编队偏移字典
        alpha_tmp -- 步长参数
        lambda_tmp -- 正则化参数
        i -- 当前智能体索引
        k -- 当前时间步
        h_next_tmp -- 下一时刻编队偏移
        
        返回:
        xhat -- 更新后的状态估计
        """
        # global z_tmp, a_ttmp
        a_ttmp = alpha_tmp
        h_tmp = AGENT_h_tmp.get(i, np.zeros((2,1)))
        X = np.array([[6], [4]])

        # print(AGENT_X_tmp.get(i, np.zeros((2,1))),h_tmp,i)
        # 1. 邻居的弹性加权信息
        eij = []
        for j in Neighbor_tmp:
            xi = AGENT_X_tmp.get(i, np.zeros((2,1))) - h_tmp
            xj = AGENT_X_tmp.get(j, np.zeros((2,1))) - AGENT_h_tmp.get(j, np.zeros((2,1)))
            # print(AGENT_X_tmp.get(j, np.zeros((2,1))),AGENT_h_tmp.get(j, np.zeros((2,1))))
            # print(xi, xj)
            eij.append((xi - xj))
        
        distances = [np.linalg.norm(e) for e in eij]
        sorted_indices = np.argsort(distances)
        c_tmp = max(1, len(Neighbor_tmp) - bar_M)
        e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)

        # 2. 辅助向量更新
        df = self.function_dfL(i, AGENT_X_tmp.get(i, np.zeros((2,1))), k ,dt)
        z_tmp = x_tmp - h_tmp +- alpha_tmp * (df + lambda_tmp * e_bar)

        # 3. 近端算子优化
        a = 0.05
        b = 1 / (2 * a_ttmp)
        bounds = [(0, 6), (0, 4)]  # 状态约束

        # 定义优化目标函数
        def objective(r):
            return a * np.linalg.norm(r) + b * np.sum((r - z_tmp)**2)

        # 使用SLSQP算法进行优化
        initial_guess = np.array([1, 1])
        result = minimize(objective, initial_guess, method='SLSQP', bounds=bounds)
        ropt_tmp = result.x.reshape(-1,1)
        ropt_tmp = ropt_tmp.flatten()
        # print(ropt_tmp+ h_next_tmp)
        return ropt_tmp + h_next_tmp

    def function_dfL(self, i, x, k, dt):
        """
        计算本地目标函数的次梯度
        
        参数:
        i -- 智能体索引号 (int)
        x -- 当前状态 (numpy.ndarray, shape=(2,1))
        k -- 当前时间步 (int)
        
        返回:
        df -- 次梯度 (numpy.ndarray, shape=(2,1))
        """
        T = 40
        # 计算基础参考位置
        xc_L = np.array([
            3 + 3 * np.sin(-(np.pi/2) * (4/T) * k * dt),
            2 + 2 * np.sin(-np.pi * (4/T) * k * dt)
        ])
        # 添加个体偏移量
        offset = np.array([
            (-0.015 + 0.03 * (i+1)/7) * np.sin(np.pi * k * dt / 40),
            (-0.02 + 0.04 * (i+1)/7) * np.sin(np.pi * k * dt / 20)
        ])
        xc_L += offset
        # 计算次梯度
        df = x - xc_L
        
        return df

    def function_update_malicious(self, bar_M, x_tmp, AGENT_X_tmp, Neighbor_ttmp, AGENT_h_tmp, i, h_next_tmp):
        """
        恶意智能体的估计值更新函数
        
        参数:
        bar_M -- 弹性邻居数量
        x_tmp -- 当前状态估计 (numpy.ndarray, shape=(2,1))
        AGENT_X_tmp -- 邻居状态字典 {邻居索引: 状态值}
        Neighbor_ttmp -- 有效邻居索引列表
        AGENT_h_tmp -- 编队偏移字典 {智能体索引: 偏移值}
        i -- 当前智能体索引
        h_next_tmp -- 下一时刻编队偏移 (numpy.ndarray, shape=(2,1))
        
        返回:
        xhat -- 更新后的状态估计 (numpy.ndarray, shape=(2,1))
        """
        # 获取当前编队偏移
        h_tmp = AGENT_h_tmp.get(i, np.zeros((2,1)))
        # print(x_tmp ,h_tmp)
        
        # 1. 计算邻居的弹性加权信息
        eij = []
        for j in Neighbor_ttmp:
            xi = x_tmp - h_tmp
            xj = AGENT_X_tmp.get(j, np.zeros((2,1))) - AGENT_h_tmp.get(j, np.zeros((2,1)))
            # print(AGENT_X_tmp.get(j, np.zeros((2,1))),AGENT_h_tmp.get(j, np.zeros((2,1))))
            # print(xi, xj)
            eij.append((xi - xj))
        
        # 处理空邻居情况
        distances = [np.linalg.norm(e) for e in eij]
        sorted_indices = np.argsort(distances)
        # e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)

        # 计算有效邻居数量
        c_tmp = max(0, len(Neighbor_ttmp) - (bar_M - 1))
        
        if c_tmp > 0:
            # 取前c_tmp个最近邻居
            # print(eij)
            e_bar = np.array(np.sum([eij[idx] for idx in sorted_indices[:c_tmp]], axis=0) / c_tmp)
        else:
            e_bar = np.zeros((2,1))
        
        # 2. 更新估计值
        xhat = (x_tmp - h_tmp) + h_next_tmp - e_bar
        
        return xhat

    def function_control_1(self, x_tmp,xhat_tmp,umax,dt):
        # FUNCTION_CONTROL_1 一阶智能体控制器
        u_tmp=(1/dt)*(xhat_tmp-x_tmp)
        # print(u_tmp)
        for un in range(len(u_tmp)):
            if u_tmp[un]>umax:
                u_tmp[un]=umax
            elif u_tmp[un]<-umax:
                u_tmp[un]=-umax

        return u_tmp

    def function_control_2(self, x_tmp,v_tmp,x_tmp2,xhat2,umax,vmax,B,dt):

        # 控制协议
        u_tmp=(1/(B*dt**2))*(xhat2-2*x_tmp2+x_tmp)
        # 控制协议限幅
        v_tmp2=v_tmp+B*u_tmp*dt
        for vn in range(len(v_tmp2)):
            if v_tmp2[vn]>vmax:
                u_tmp[vn]=(vmax-v_tmp[vn])/(B*dt)
            elif v_tmp2[vn]<-vmax:
                u_tmp[vn]=(-vmax-v_tmp[vn])/(B*dt)

        for un in range(len(u_tmp)):
            # print(un)
            if u_tmp[un]>umax:
                u_tmp[un]=umax
            elif u_tmp[un]<-umax:
                u_tmp[un]=-umax

        return u_tmp

    def function_trans_1(self, x,u,dt):
        x1=x+u*dt
        v1=(x1-x)/dt
        return x1,v1

    def function_trans_2(self, x,v,u,dt):
        x1=x+v*dt
        v1=v+u*dt
        return x1,v1
    
    def uav1_px4state_cb(self, msg):
        id = 1
        self.px4_state_[id-1] = msg
        # print((not self.px4_state_.armed))

    def uav2_px4state_cb(self, msg):
        id = 2
        self.px4_state_[id-1] = msg

    # def uav3_px4state_cb(self, msg):
    #     id = 3
    #     self.px4_state_[id-1] = msg

    def uav1_arm(self,):
        if self.uav1_armService(True):
            print('uav1--已解锁！')
            return True
        else:
            print("uav1--解锁失败!")
            return False
        
    def uav2_arm(self,):
        if self.uav2_armService(True):
            print('uav2--已解锁！')
            return True
        else:
            print("uav2--解锁失败!")
            return False
        
    # def uav3_arm(self,):
    #     if self.uav3_armService(True):
    #         print('uav3--已解锁！')
    #         return True
    #     else:
    #         print("uav3--解锁失败!")
    #         return False
        
    def pubPxyPzCmd(self, cmd_p, id):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = 1    # pub in ENU local frame
        msg.type_mask = 0b110111111000  # pub pz+py+px. ignore yaw and yaw rate
        msg.position.x = cmd_p[0]   # pub px
        msg.position.y = cmd_p[1]   # pub py
        msg.position.z = cmd_p[2]   # pub local z altitude setpoint
        if id == 1:
            self.uav1_target_pose_pub_.publish(msg)
        elif id == 2:
            self.uav2_target_pose_pub_.publish(msg)
        # elif id == 3:
        #     self.uav3_target_pose_pub_.publish(msg)

    def pubVxyPzYawCmd(self, cmd_sp, yaw_sp, id):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = 1    # pub in ENU local frame;
        msg.type_mask = 0b100111000011  # pub vx+vy+vz+pz+yaw, vz is feedforward vel.
        msg.velocity.x = cmd_sp[0]  # pub vx
        msg.velocity.y = cmd_sp[1]  # pub vy
        msg.velocity.z = 0  # pub vz
        msg.position.z = cmd_sp[2]  # pub local z altitude setpoint
        msg.yaw = yaw_sp
        if id == 1:
            self.uav1_target_pose_pub_.publish(msg)
        elif id == 2:
            self.uav2_target_pose_pub_.publish(msg)
        # elif id == 3:
        #     self.uav3_target_pose_pub_.publish(msg)

    def pubPxyzVxyzYawCmd(self, cmd_p, cmd_v, yaw_sp, id):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = 1    # pub in ENU local frame;
        msg.type_mask = 0b100111000000  # pub vz+vy+vx+pz+py+px+yaw, vz is feedforward vel.
        msg.position.x = cmd_p[0]   # pub px
        msg.position.y = cmd_p[1]   # pub py
        msg.position.z = cmd_p[2]   # pub local z altitude setpoint
        msg.velocity.x = cmd_v[0]   # pub vx
        msg.velocity.y = cmd_v[1]   # pub vy
        msg.velocity.z = 0          # pub vz
        msg.yaw = yaw_sp
        if id == 1:
            self.uav1_target_pose_pub_.publish(msg)
        elif id == 2:
            self.uav2_target_pose_pub_.publish(msg)
        # elif id == 3:
        #     self.uav3_target_pose_pub_.publish(msg)

    def pubPxyVzCmd(self, cmd_p, cmd_v, id):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = 1    # pub in ENU local frame
        msg.type_mask = 0b110111011100  # pub vz+py+px. ignore yaw and yaw rate
        msg.position.x = cmd_p[0]   # pub px
        msg.position.y = cmd_p[1]   # pub py
        msg.velocity.z = cmd_v[2]   # pub vz
        if id == 1:
            self.uav1_target_pose_pub_.publish(msg)
        elif id == 2:
            self.uav2_target_pose_pub_.publish(msg)
        # elif id == 3:
        #     self.uav3_target_pose_pub_.publish(msg)

    def pubPzAxyYawCmd(self, cmd_p, cmd_a, yaw_sp, id):
        msg = PositionTarget()
        msg.header.stamp = rospy.Time.now()
        msg.coordinate_frame = 1    # pub in ENU local frame
        msg.type_mask = 0b100100111011  # pub ay+ax+pz. ignore yaw and yaw rate
        msg.position.z = cmd_p[2]   # pub pz
        msg.acceleration_or_force.x = cmd_a[0]   # pub ax
        msg.acceleration_or_force.y = cmd_a[1]   # pub ay
        msg.yaw = yaw_sp
        if id == 1:
            self.uav1_target_pose_pub_.publish(msg)
        elif id == 2:
            self.uav2_target_pose_pub_.publish(msg)
        # elif id == 3:
        #     self.uav3_target_pose_pub_.publish(msg)

    def uav1_mavpose_cb(self, msg):
        id = 1
        mavPos_all = [0.0, 0.0, 0.0]
        mavPos_all[0] = msg.pose.position.x
        mavPos_all[1] = msg.pose.position.y
        mavPos_all[2] = msg.pose.position.z
        self.uav_mavPos[3*(id-1):id*3] = np.array(mavPos_all)
        mavAtt_ = msg.pose.orientation
        eulerAngle4 = self.quat_to_euler(mavAtt_)

    def uav2_mavpose_cb(self, msg):
        id = 2
        mavPos_all = [0.0, 0.0, 0.0]
        mavPos_all[0] = msg.pose.position.x
        mavPos_all[1] = msg.pose.position.y
        mavPos_all[2] = msg.pose.position.z
        self.uav_mavPos[3*(id-1):id*3] = np.array(mavPos_all)
        mavAtt_ = msg.pose.orientation
        eulerAngle4 = self.quat_to_euler(mavAtt_)

    # def uav3_mavpose_cb(self, msg):
    #     id = 3
    #     mavPos_all = [0.0, 0.0, 0.0]
    #     mavPos_all[0] = msg.pose.position.x
    #     mavPos_all[1] = msg.pose.position.y
    #     mavPos_all[2] = msg.pose.position.z
    #     self.uav_mavPos[3*(id-1):id*3] = np.array(mavPos_all)
    #     mavAtt_ = msg.pose.orientation
    #     eulerAngle4 = self.quat_to_euler(mavAtt_)

    def vel_control(self, now_3d, exp_3d, now_ang, exp_ang, model):
        w_P = self.w_P if model == 'ugv' else -self.w_P
        vel = Twist()
        vel.linear.y = min(max(self.vel_Py * (exp_3d[1] - now_3d[1]),self.vel_thres_min),self.vel_thres_max)
        vel.linear.x = min(max(self.vel_Px * (exp_3d[0] - now_3d[0]),self.vel_thres_min),self.vel_thres_max)
        vel.angular.z = w_P * (exp_ang - now_ang) 
        return vel
    
    def vel_control_PD(self, now_pos, exp_pos, now_vel, exp_vel, now_ang, exp_ang, model):
        w_P = self.w_P if model == 'ugv' else -self.w_P
        vel = Twist()
        if len(exp_pos)!=0 and len(exp_vel)!=0:
            vel_x = self.vel_Px * (exp_pos[0] - now_pos[0]) + self.vel_Dx * (exp_vel[0] - now_vel[0])
            vel_y = self.vel_Py * (exp_pos[1] - now_pos[1]) + self.vel_Dy * (exp_vel[1] - now_vel[1])
        else:
            vel_x = 0
            vel_y = 0

        vel.linear.y = min(max(vel_y,self.vel_thres_min),self.vel_thres_max)
        vel.linear.x = min(max(vel_x,self.vel_thres_min),self.vel_thres_max)
        vel.angular.z = w_P * (exp_ang - now_ang) 
        return vel

    def vel_control_PD2(self, now_pos, exp_pos, now_vel, exp_vel, now_ang, exp_ang, model):
        w_P = self.w_P if model == 'ugv' else -self.w_P
        vel = Twist()
        vel_x = self.vel_Px2 * (exp_pos[0] - now_pos[0]) + self.vel_Dx2 * (exp_vel[0] - now_vel[0])
        vel_y = self.vel_Py2 * (exp_pos[1] - now_pos[1]) + self.vel_Dy2 * (exp_vel[1] - now_vel[1])

        vel.linear.y = min(max(vel_y,self.vel_thres_min),self.vel_thres_max)
        vel.linear.x = min(max(vel_x,self.vel_thres_min),self.vel_thres_max)
        vel.angular.z = w_P * (exp_ang - now_ang) 
        return vel
    
    def np2_to_Twist(self, vel, np2):
        vel.linear.x = min(max(np2[0],self.vel_thres_min),self.vel_thres_max)
        vel.linear.y = min(max(np2[1],self.vel_thres_min),self.vel_thres_max)
        return vel

    def command_callback(self, msg):
        self.command = msg.data
        # print(command)
        print('Receive command  ', msg.data)
        if self.command == 0:
            self.state = 'UGV_STOP'
        if self.command == 1:
            self.state = 'UAV_UGV_STOP'
        elif self.command == 2:
            self.state = 'RESET'
        elif self.command == 3:
            self.state = 'PREPARE'
            print(self.state)
        elif self.command == 4:
            self.state = 'TAKEOFF'
        elif self.command == 5:
            self.state = 'ALGRITHM'
        elif self.command == 6:
            self.state = 'LAND'

    def uav1_pose_callback(self, msg):
        id = 1
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.uav_pos[(id-1)*3:(id-1)*3+3] = msg_np[0:3]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.uav_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def uav1_vel_callback(self, msg):
        id = 1
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.uav_vel[(id-1)*3:(id-1)*3+3] = msg_np[0:3]

    def uav2_pose_callback(self, msg):
        id = 2
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.uav_pos[(id-1)*3:(id-1)*3+3] = msg_np[0:3]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.uav_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def uav2_vel_callback(self, msg):
        id = 2
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.uav_vel[(id-1)*3:(id-1)*3+3] = msg_np[0:3]

    # def uav3_pose_callback(self, msg):
    #     id = 3
    #     # msg_np = self.point2np(msg.position)
    #     msg_np = self.point2np(msg.pose.position)
    #     self.uav_pos[(id-1)*3:(id-1)*3+3] = msg_np[0:3]
    #     # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
    #     (_, _, self.uav_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    # def uav3_vel_callback(self, msg):
    #     id = 3
    #     # msg_np = self.point2np(msg.linear)
    #     msg_np = self.point2np(msg.twist.linear)
    #     self.uav_vel[(id-1)*3:(id-1)*3+3] = msg_np[0:3]

    def ugv1_pose_callback(self, msg):
        id = 1
        # print(msg)
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.ugv_pos[(id-1)*2:(id-1)*2+2] = msg_np[0:2]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def ugv1_vel_callback(self, msg):
        id = 1
        # print(msg)
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.ugv_vel[(id-1)*2:(id-1)*2+2] = msg_np[0:2]

    def ugv2_pose_callback(self, msg):
        id = 2
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.ugv_pos[(id-1)*2:(id-1)*2+2] = msg_np[0:2]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def ugv2_vel_callback(self, msg):
        id = 2
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.ugv_vel[(id-1)*2:(id-1)*2+2] = msg_np[0:2]

    def ugv3_pose_callback(self, msg):
        id = 3
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.ugv_pos[(id-1)*2:(id-1)*2+2] = msg_np[0:2]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def ugv3_vel_callback(self, msg):
        id = 3
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.ugv_vel[(id-1)*2:(id-1)*2+2] = msg_np[0:2]

    def ugv4_pose_callback(self, msg):
        id = 4
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.ugv_pos[(id-1)*2:(id-1)*2+2] = msg_np[0:2]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def ugv4_vel_callback(self, msg):
        id = 4
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.ugv_vel[(id-1)*2:(id-1)*2+2] = msg_np[0:2]

    def ugv5_pose_callback(self, msg):
        id = 5
        # msg_np = self.point2np(msg.position)
        msg_np = self.point2np(msg.pose.position)
        self.ugv_pos[(id-1)*2:(id-1)*2+2] = msg_np[0:2]
        # (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.orientation)
        (_, _, self.ugv_yaw[id-1]) = self.quat_to_euler(msg.pose.orientation)

    def ugv5_vel_callback(self, msg):
        id = 5
        # msg_np = self.point2np(msg.linear)
        msg_np = self.point2np(msg.twist.linear)
        self.ugv_vel[(id-1)*2:(id-1)*2+2] = msg_np[0:2]

    def quat_to_euler(self, orientation):
        x = orientation.x
        y = orientation.y
        z = orientation.z
        w = orientation.w
        r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
        p = math.asin(2 * (w * y - x * z))
        y = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
        rpy = [r, p, y]
        return rpy

    def point2np(self, point):
        db_K = 1
        x = point.x / db_K
        y = point.y / db_K
        z = point.z / db_K
        return np.array([x, y, z])

    def stop(self):
        self.ugv1_vel_pub.publish(Twist())
        self.ugv2_vel_pub.publish(Twist())
        self.ugv3_vel_pub.publish(Twist())
        self.ugv4_vel_pub.publish(Twist())
        self.ugv5_vel_pub.publish(Twist())
        # self.ugv4_vel_pub.publish(Twist())
    
    def write_log(self, time, folder_path):
        '''
        write log_variables in folder_path in .txt seperately
        '''
        variables_dict = {
            'current_step': np.array([self.current_step]),
            'command': np.array([self.command]),
            'ugv_pos': self.ugv_pos,
            'ugv_vel': self.ugv_vel,
            'AGENT': self.AGENT,
            'AGENT_X': self.AGENT_X,
            'AGENT_V': self.AGENT_V
        }

        for name, variable in variables_dict.items():
            file_path = folder_path + '/' + name + '.txt'
            with open(file_path,'a') as file:
                # concatenate time and data column
                # print(name, variable, np.array([time]))
                data = np.concatenate((np.array([time]), variable))
                # save data, seperated by bland space, format is float
                np.savetxt( file, data, newline=' ', fmt='%f')
                file.write('\n') # newline


if __name__ == '__main__':
    ############# create the log file folder #############
    folder = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
    log_path = folder + '/data_log/log' + datetime.now().strftime("%Y%m%d") + \
    '-' + datetime.now().strftime("%H%M%S")
    os.mkdir(log_path) # do not use os.makedirs() for error reporting
    ############# 石哲鑫-一般线性多智能体预设时间编队跟踪 #############
    control_node = zrq_control(log_path)
    try:
        control_node.main()
    except rospy.ROSInterruptException:
        pass

